﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.IO.Ports;
using System.IO;
using System.Runtime.InteropServices;
using System.Globalization;
using System.Windows.Forms;

using csmatio.types;
using csmatio.io;

namespace GroundStation
{
    class Telemetry
    {



        // time in ms 
        UInt32 time;

        UInt32 dev_aviable;

        // imu
        float imu_acc_x;
        float imu_acc_y;
        float imu_acc_z;
        float imu_ars_x;
        float imu_ars_y;
        float imu_ars_z;
        float imu_acc_mod1_x;
        float imu_acc_mod1_y;
        float imu_acc_mod2_x;
        float imu_acc_mod2_y;
        float imu_pitch;
        float imu_roll;

        // tilt sensor
        float tlt_pitch;
        float tlt_roll;

        // air data
        float adc_alt;
        float adc_air_speed;
        float adc_vario;
        float adc_temp;
        float adc_stat_press;
        float adc_dyn_press;

        // gps 1
        float gps1_lat;
        float gps1_lon;
        float gps1_alt;
        float gps1_speed;
        float gps1_vdop;
        float gps1_hdop;

        // gps 2
        float gps2_lat;
        float gps2_lon;
        float gps2_alt;
        float gps2_speed;
        float gps2_vdop;
        float gps2_hdop;

        // magnetometer
        float mag_x;
        float mag_y;
        float mag_z;

        // ultrasonic range finder 1
        float usr1_range;

        // ultrasonic range finder 2
        float usr2_range;

        
        SerialPort serialPort = new SerialPort();
        //string logSTime;

        string logFileName = "";

        Thread thread;
        Mutex mutex = new Mutex();
        bool keepGoing = false;
        private bool _saveASCI = false;
        int lastTicks = 0;

        private static int coremodules = 39;    // count of data from core modules
        private static int maxstudentmodules = 20; // maximul data count from students modules

        // student data
        float[] std_data = new float[maxstudentmodules];

        // getters
        public UInt32 Time
        {
            get
            {
                UInt32 ret;

                mutex.WaitOne();
                ret = time;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public UInt32 DevAviable
        {
            get
            {
                UInt32 ret;

                mutex.WaitOne();
                ret = dev_aviable;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuAccX
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_acc_x;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuAccY
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_acc_y;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuAccZ
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_acc_z;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuArsX
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_ars_x;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuArsY
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_ars_y;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuArsZ
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_ars_z;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuAccMod1X
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_acc_mod1_x;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuAccMod1Y
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_acc_mod1_y;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuAccMod2X
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_acc_mod2_x;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuAccMod2Y
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_acc_mod2_y;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuPitch
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_pitch;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float ImuRoll
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = imu_roll;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float TltPitch
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = tlt_pitch;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float TltRoll
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = tlt_roll;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps1Lat
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps1_lat;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps1Lon
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps1_lon;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps1Alt
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps1_alt;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps1Speed
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps1_speed;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps1VDOP
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps1_vdop;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps1HDOP
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps1_hdop;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps2Lat
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps2_lat;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps2Lon
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps2_lon;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps2Alt
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps2_alt;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps2Speed
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps2_speed;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps2VDOP
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps2_vdop;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Gps2HDOP
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = gps2_hdop;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float MagX
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = mag_x;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float MagY
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = mag_y;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float MagZ
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = mag_z;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float AdcAlt
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = adc_alt;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float AdcAirSpeed
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = adc_air_speed;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float AdcVario
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = adc_vario;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float AdcTemp
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = adc_temp;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float AdcStatPress
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = adc_stat_press;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float AdcDynPress
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = adc_dyn_press;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Usr1Range
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = usr1_range;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float Usr2Range
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = usr2_range;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public string StudentData
        {
            get
            {
                string ret = "";

                mutex.WaitOne();
                for (int i = 0; i < maxstudentmodules; i++)
                {
                    ret = ret + std_data[i].ToString("0.00000", CultureInfo.InvariantCulture) + "\t";
                }
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public bool Init(string port, bool saveASCI)
        {
            _saveASCI = saveASCI;
            if (Init(port))
                return true;
            else
                return false;
        }


        public bool Init(string port)
        {
            System.Console.Write("telelmetry: opening port {0}...", port);
            lastTicks = Environment.TickCount;
            thread = new Thread(Proc);
            // serial port configuration
            serialPort.PortName = port;
            serialPort.BaudRate = 230400;//230400;// 403200;// 312500;//230400;
            serialPort.WriteTimeout = 1000;
            serialPort.ReadTimeout = 1000;
            try
            {
                serialPort.Open();
            }
            catch (IOException)
            {
                System.Windows.Forms.MessageBox.Show("Cannot open " + port + "!", "Error", System.Windows.Forms.MessageBoxButtons.OK, System.Windows.Forms.MessageBoxIcon.Error);
                System.Console.WriteLine("failed!", port);
                return false;
            }
            System.Console.WriteLine("ok", port);

            logFileName = DateTime.Now.ToString("yyyy-MM-dd_HHmmss");// +".log";

            keepGoing = true;
            thread.Start();
            return true;
        }

        public void Stop()
        {
            if (keepGoing == true)
            {
                keepGoing = false;
                thread.Join(200); // ?? sekani aplikace - predelat
            }
        }

        private void Proc()
        {
            byte[] buf = new byte[1000];
            int tmp;
            int len;
            int index = 0;
            byte sum = 0;
            FileStream fs = null;
            StreamWriter sw = null;
            if (_saveASCI)
            {
                fs = new FileStream("MS_" + logFileName + ".log", FileMode.Append, FileAccess.Write, FileShare.ReadWrite);
                sw = new StreamWriter(fs);
            }
            //FileStream fs = new FileStream(logFileName + ".log", FileMode.Append, FileAccess.Write, FileShare.ReadWrite);
            /// <sumary>
            /// Matlab .mat file variables
            /// </sumary>
            List<float[]> mDataList = new List<float[]>(); // list of floats - messages from modsys
            int matI;// writing index

            while (keepGoing)
            {
                try
                {
                    tmp = serialPort.ReadByte();
                    if (tmp == 0xAB)
                    {
                        len = serialPort.ReadByte();
                        if (len > 0 && len >= (coremodules*4))
                        {
                            sum = 0;
                            for (int i = 0; i < len; i++)
                            {
                                tmp = serialPort.ReadByte();
                                if (tmp >= 0)
                                {
                                    buf[i] = (byte)tmp;
                                    sum += (byte)tmp;
                                }
                                else
                                {
                                    System.Console.WriteLine("telemetry: wrong data!");
                                    continue;
                                }
                            }
                        }
                        else
                        {
                            System.Console.WriteLine("telemetry: wrong length!");
                        }
                        tmp = serialPort.ReadByte();
                        if (tmp >= 0)
                        {
                            sum -= (byte)tmp;

                            if (sum == 0)
                            {
                                mutex.WaitOne();

                                index = 0;
                                matI = 0;
                                time = ConvertUint32Data(ref buf, index);

                                /// <sumary>
                                /// float vector for message data
                                /// </sumary>
                                float[] matData = new float[coremodules + maxstudentmodules]; // mat row/message data
                                matData[matI++] = time;

                                index += 4;
                                dev_aviable = ConvertUint32Data(ref buf, index);
                                matData[matI++] = dev_aviable;
                                index += 4;
                                imu_acc_x = ConvertFloatData(ref buf, index); matData[matI++] = imu_acc_x;
                                index += 4;
                                imu_acc_y = ConvertFloatData(ref buf, index); matData[matI++] = imu_acc_y;
                                index += 4;
                                imu_acc_z = ConvertFloatData(ref buf, index); matData[matI++] = imu_acc_z;
                                index += 4;
                                imu_ars_x = ConvertFloatData(ref buf, index); matData[matI++] = imu_ars_x;
                                index += 4;
                                imu_ars_y = ConvertFloatData(ref buf, index); matData[matI++] = imu_ars_y;
                                index += 4;
                                imu_ars_z = ConvertFloatData(ref buf, index); matData[matI++] = imu_ars_z;
                                index += 4;
                                imu_acc_mod1_x = ConvertFloatData(ref buf, index); matData[matI++] = imu_acc_mod1_x;
                                index += 4;
                                imu_acc_mod1_y = ConvertFloatData(ref buf, index); matData[matI++] = imu_acc_mod1_y;
                                index += 4;
                                imu_acc_mod2_x = ConvertFloatData(ref buf, index); matData[matI++] = imu_acc_mod2_x;
                                index += 4;
                                imu_acc_mod2_y = ConvertFloatData(ref buf, index); matData[matI++] = imu_acc_mod2_y;
                                index += 4;
                                imu_pitch = ConvertFloatData(ref buf, index); matData[matI++] = imu_pitch;
                                index += 4;
                                imu_roll = ConvertFloatData(ref buf, index); matData[matI++] = imu_roll;

                                index += 4;
                                tlt_pitch = ConvertFloatData(ref buf, index); matData[matI++] = tlt_pitch;
                                index += 4;
                                tlt_roll = ConvertFloatData(ref buf, index); matData[matI++] = tlt_roll;

                                index += 4;
                                gps1_lat = ConvertFloatData(ref buf, index); matData[matI++] = gps1_lat;
                                index += 4;
                                gps1_lon = ConvertFloatData(ref buf, index); matData[matI++] = gps1_lon;
                                index += 4;
                                gps1_alt = ConvertFloatData(ref buf, index); matData[matI++] = gps1_alt;
                                index += 4;
                                gps1_speed = ConvertFloatData(ref buf, index); matData[matI++] = gps1_speed;
                                index += 4;
                                gps1_vdop = ConvertFloatData(ref buf, index); matData[matI++] = gps1_vdop;
                                index += 4;
                                gps1_hdop = ConvertFloatData(ref buf, index); matData[matI++] = gps1_hdop;

                                index += 4;
                                gps2_lat = ConvertFloatData(ref buf, index); matData[matI++] = gps2_lat;
                                index += 4;
                                gps2_lon = ConvertFloatData(ref buf, index); matData[matI++] = gps2_lon;
                                index += 4;
                                gps2_alt = ConvertFloatData(ref buf, index); matData[matI++] = gps2_alt;
                                index += 4;
                                gps2_speed = ConvertFloatData(ref buf, index); matData[matI++] = gps2_speed;
                                index += 4;
                                gps2_vdop = ConvertFloatData(ref buf, index); matData[matI++] = gps2_vdop;
                                index += 4;
                                gps2_hdop = ConvertFloatData(ref buf, index); matData[matI++] = gps2_hdop;

                                index += 4;
                                mag_x = ConvertFloatData(ref buf, index); matData[matI++] = mag_x;
                                index += 4;
                                mag_y = ConvertFloatData(ref buf, index); matData[matI++] = mag_y;
                                index += 4;
                                mag_z = ConvertFloatData(ref buf, index); matData[matI++] = mag_z;

                                index += 4;
                                adc_alt = ConvertFloatData(ref buf, index); matData[matI++] = adc_alt;
                                index += 4;
                                adc_air_speed = ConvertFloatData(ref buf, index); matData[matI++] = adc_air_speed;
                                index += 4;
                                adc_vario = ConvertFloatData(ref buf, index); matData[matI++] = adc_vario;
                                index += 4;
                                adc_temp = ConvertFloatData(ref buf, index); matData[matI++] = adc_temp;
                                index += 4;
                                adc_stat_press = ConvertFloatData(ref buf, index); matData[matI++] = adc_stat_press;
                                index += 4;
                                adc_dyn_press = ConvertFloatData(ref buf, index); matData[matI++] = adc_dyn_press;

                                index += 4;
                                usr1_range = ConvertFloatData(ref buf, index); matData[matI++] = usr1_range;
                                index += 4;
                                usr2_range = ConvertFloatData(ref buf, index); matData[matI++] = usr2_range;

                                if (len > (coremodules * 4))
                                {
                                    int length = len;
                                    int ind = 0;

                                    if (length > (coremodules + maxstudentmodules) * 4) length = (coremodules + maxstudentmodules) * 4;
                                    for (int i = 0; i < length - (coremodules * 4); i += 4)
                                    {
                                        std_data[ind] = ConvertFloatData(ref buf, (coremodules * 4) + i);
                                        matData[matI++] = std_data[ind]; // add student data to Matlab float vector - will be saved to .mat file
                                        ind++;
                                    }

                                }
                                mDataList.Add(matData); // add Matlab float vector to list of data - will be saved to .mat file

                                /*using (FileStream fs = new FileStream(logFileName + ".log", FileMode.Append, FileAccess.Write, FileShare.ReadWrite))
                                {
                                    using (StreamWriter sw = new StreamWriter(fs))
                                    {*/
                                if (_saveASCI)
                                {
                                    string str = time.ToString() + "\t" +
                                     dev_aviable.ToString() + "\t" +

                                     imu_acc_x.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_acc_y.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_acc_z.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_ars_x.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_ars_y.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_ars_z.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_acc_mod1_x.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_acc_mod1_y.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_acc_mod2_x.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_acc_mod2_y.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_pitch.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     imu_roll.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +

                                     tlt_pitch.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     tlt_roll.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +

                                     gps1_lat.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps1_lon.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps1_alt.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps1_speed.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps1_vdop.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps1_hdop.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +

                                     gps2_lat.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps2_lon.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps2_alt.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps2_speed.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps2_vdop.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     gps2_hdop.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +

                                     mag_x.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     mag_y.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     mag_z.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +

                                     adc_alt.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     adc_air_speed.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     adc_vario.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     adc_temp.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     adc_stat_press.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     adc_dyn_press.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +

                                     usr1_range.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +
                                     usr2_range.ToString("0.00000", CultureInfo.InvariantCulture) + "\t" +

                                     this.StudentData;

                                    sw.WriteLine(str);
                                }
                                mutex.ReleaseMutex();
                            }
                            else
                            {
                                System.Console.WriteLine("Wrong checksum in received data.");
                            }
                        }
                        else
                        {
                            System.Console.WriteLine("telemetry: checksum failed!");
                            continue;
                        }
                    }
                }
                //}
                //                      }
                catch (IOException)
                {
                    System.Console.WriteLine("telemetry: io exception!");
                }
                catch (TimeoutException)
                {
                    System.Console.WriteLine("telemetry: com timeout!");
                }
                catch (FormatException)
                {
                    System.Console.WriteLine("telemetry: data corrupted!");
                }
                catch (ThreadAbortException)
                {
                    return;
                }
            }


            /// <sumary>
            /// Create .mat file data output if there is nonempty float list
            /// </sumary>
            if (mDataList.Count > 0) // list empty check
            {

                float[][] mlData = mDataList.ToArray(); // list to array conversion - possible improvement in future
                MLSingle mlSingle = new MLSingle("data", mlData); // creates matlab array of single values as "data" variable
                List<MLArray> list = new List<MLArray>(); // list of matlab array
                list.Add(mlSingle); // adding data to matlab list
                new MatFileWriter("MS_" + logFileName + ".mat", list, false); // creates .mat file: string filename, array data, bool compression
            }
            serialPort.Close();
            try
            {
                //if (sw != null) sw.Close();
                if (_saveASCI)
                {
                    sw.Close();
                    fs.Close();
                }
            }
            catch (Exception e)
            {
                System.Console.WriteLine("Error: " + e.ToString());
            }
        }

        private float ConvertFloatData(ref byte[] buf, int index)
        {
            byte[] val = new byte[4];
            float ret = 0;

            try
            {
                val[0] = buf[index + 0];
                val[1] = buf[index + 1];
                val[2] = buf[index + 2];
                val[3] = buf[index + 3];

                ret = BitConverter.ToSingle(val, 0);
            }
            catch (Exception)
            { }

            return ret;
        }

        private UInt32 ConvertUint32Data(ref byte[] buf, int index)
        {
            byte[] val = new byte[4];
            UInt32 ret = 0;

            try
            {
                val[0] = buf[index + 0];
                val[1] = buf[index + 1];
                val[2] = buf[index + 2];
                val[3] = buf[index + 3];

                ret = BitConverter.ToUInt32(val, 0);
            }
            catch (Exception)
            { }

            return ret;
        }
    }
}

